module FES1DL23_m
use FES1DL2_m
use Geometry_m

type, extends(FES1DL2) :: FES1DL23
contains
    procedure :: initFES => initFES1DL23
    
    procedure :: assembleBeamStiffL23_func
    procedure :: assembleBeamStiffL23_const
    generic :: assembleBeamStiff => assembleBeamStiffL23_func, assembleBeamStiffL23_const

    procedure :: assemblePressureL23_func
    procedure :: assemblePressureL23_const
    generic :: assemblePressure => assemblePressureL23_func, assemblePressureL23_const
    
    procedure :: assembleLumpForceL23_const
    generic :: assembleLumpForce => assembleLumpForceL23_const
    
    procedure :: applyBCZeroL23_MR
    procedure :: applyBCZeroL23_M
    procedure :: applyBCZeroL23_R
    generic :: applyBCZero => applyBCZeroL23_MR, applyBCZeroL23_M, applyBCZeroL23_R
    
    procedure :: applyBCL23_const
    procedure :: applyBCL23_point
    generic :: applyBC => applyBCL23_const, applyBCL23_point
end type

contains

subroutine initFES1DL23(this, mesh1D)
    implicit none
    class(FES1DL23), intent(out) :: this
    type(mesh1DL2), target, intent(in) :: mesh1D

    integer :: band
    this%dim = 1
    this%nDof = mesh1D%nv * 3
    this%nVertices = mesh1D%nv
    this%nElement = mesh1D%nt
    this%nBoundary = mesh1D%nb
    this%nPerElement = 2
    this%nPerBoundary = 1
    this%dofPerNode = 3
    this%dofPerElement = 6
    this%dofPerBoundary = 3
    
    band = mesh1D.elementDiff()
    this%nBand = (band + 1) * 3
    this%mesh1D => mesh1D
end subroutine

subroutine assembleBeamStiffL23_func(this, funcE, funcG, funcH, time, tp, A)
    implicit none
    class(FES1DL23), intent(in) :: this
    real*8, external :: funcE, funcG, funcH
    real*8, intent(in) :: time
    integer, intent(in) :: tp
    type(smatrix), intent(inout):: A
    
    call this.assembleGlobalMatrix_function3(funcE, funcG, funcH, time, BeamStiffL23, tp, A)
end subroutine

subroutine assembleBeamStiffL23_const(this, constE, constG, constH, tp, A)
    implicit none
    class(FES1DL23), intent(in) :: this
    real*8, intent(in) :: constE, constG, constH
    integer, intent(in) :: tp
    type(smatrix), intent(inout):: A
    
    call this.assembleGlobalMatrix_const3(constE, constG, constH, BeamStiffL23, tp, A)
end subroutine

subroutine BeamStiffL23(x, E, G, H, tp, K)
    implicit none
    real*8, intent(in) :: x(2), E(2), G(2), H(2)
    integer, intent(in) :: tp
    real*8, intent(out) :: K(6, 6)
    
    real*8 :: l, mE, mG, mH, temp1, temp2, temp3, temp4, temp5, Kb(6, 6), Ks(6, 6)
    l = abs(x(2)-x(1))
    mE = 0.5D0 * sum(E)
    mG = 0.5D0 * sum(G)
    mH = 0.5D0 * sum(H)
    temp1 = mE * mH / (3.0D0 * l)
    temp2 = 0.5 * temp1
    Kb = 0.0D0
    Kb(1, 1) = temp1
    Kb(2, 1) = temp2
    Kb(4, 1) = -temp1
    Kb(5, 1) = -temp2
    Kb(1, 2) = temp2
    Kb(2, 2) = temp1
    Kb(4, 2) = -temp2
    Kb(5, 2) = -temp1
    Kb(:, 4) = -Kb(:, 1)
    Kb(:, 5) = -Kb(:, 2)
    
    temp3 = 0.25D0 * mG * l / mH
    temp4 = 0.5D0 * mG
    temp5 = mG * mH / l
    Ks = 0.0D0
    Ks(1, 1) = temp3
    Ks(2, 1) = -temp3
    Ks(3, 1) = temp4
    Ks(4, 1) = temp3
    Ks(5, 1) = -temp3
    Ks(6, 1) = -temp4
    
    Ks(1, 2) = -temp3
    Ks(2, 2) = temp3
    Ks(3, 2) = -temp4
    Ks(4, 2) = -temp3
    Ks(5, 2) = temp3
    Ks(6, 2) = temp4
    
    Ks(1, 3) = temp4
    Ks(2, 3) = -temp4
    Ks(3, 3) = temp5
    Ks(4, 3) = temp4
    Ks(5, 3) = -temp4
    Ks(6, 3) = -temp5
    
    Ks(:, 4) = Ks(:, 1)
    Ks(:, 5) = Ks(:, 2)
    Ks(:, 6) = -Ks(:, 3)
    K = Kb + Ks
end subroutine

subroutine assembleLumpForceL23_const(this, constP, node, RHS)
    implicit none
    class(FES1DL23), intent(in) :: this
    real*8, intent(in) :: constP
    integer, intent(in) :: node
    real*8, intent(inout):: RHS(:)
    
    integer :: idx
    idx = 3 * node
    RHS(idx) = RHS(idx) + constP
end subroutine
    
subroutine assemblePressureL23_func(this, funcP, time, tp, RHS)
    implicit none
    class(FES1DL23), intent(in) :: this
    real*8, external :: funcP
    real*8, intent(in) :: time
    integer, intent(in) :: tp
    real*8, intent(inout):: RHS(:)
    
    call this.assembleGlobalVector_function(funcP, time, elementPressureL23, tp, RHS)
end subroutine


subroutine assemblePressureL23_const(this, constP, tp, RHS)
    implicit none
    class(FES1DL23), intent(in) :: this
    real*8, intent(in) :: constP
    integer, intent(in) :: tp
    real*8, intent(inout):: RHS(:)
    
    call this.assembleGlobalVector_const(constP, elementPressureL23, tp, RHS)
end subroutine

subroutine elementPressureL23(x, P, tp, K)
    implicit none
    real*8, intent(in) :: x(2), P(2)
    integer, intent(in) :: tp
    real*8, intent(out) :: K(6)
    
    real*8 :: length
    length = abs(x(2) - x(1))    
    K = 0.0D0
    K(3) = (2 * P(1) + P(2)) / 6.0D0 * length
    K(6) = (P(1) + 2 * P(2)) / 6.0D0 * length
end subroutine

subroutine applyBCZeroL23_MR(this, A, RHS, label, direct)
    implicit none
    class(FES1DL23), intent(in) :: this
    type(smatrix), intent(inout):: A
    real*8, intent(inout) :: RHS(A%M)
    integer, intent(in) :: label(:)
    integer, intent(in), optional :: direct

    type(LinkedSet) :: dirichletBC
    call this.mesh1D.extractBoundaryNodes(dirichletBC, label)
    if (present(direct)) then
        call this.applyBCZero0_MR(A, RHS, dirichletBC, direct)
    else
        call this.applyBCZero0_MR(A, RHS, dirichletBC)
    end if
	call dirichletBC.empty()
end subroutine


subroutine applyBCZeroL23_M(this, A, label, direct)
    use Set_m
	implicit none
	class(FES1DL23), intent(in) :: this  
	type(SMatrix), intent(inout):: A  
	integer, intent(in) :: label(:)
    integer, intent(in), optional :: direct

	type(mesh1DL2), pointer :: mesh1D
	type(LinkedSet) :: dirichletBC
	mesh1D => this%mesh1D
	call mesh1D.extractBoundaryNodes(dirichletBC, label)
    if (present(direct)) then
        call this.applyBCZero0_M(A, dirichletBC, direct)
    else
        call this.applyBCZero0_M(A, dirichletBC)
    end if
	call dirichletBC.empty()
end subroutine

subroutine applyBCZeroL23_R(this, RHS, label, direct)
    use Set_m
	implicit none
	class(FES1DL23), intent(in) :: this      
	real*8, intent(inout) :: RHS(:) 
	integer, intent(in) :: label(:)   
    integer, intent(in), optional :: direct

	type(mesh1DL2), pointer :: mesh1D
	type(LinkedSet) :: dirichletBC
	mesh1D => this%mesh1D
	call mesh1D.extractBoundaryNodes(dirichletBC, label)
    if (present(direct)) then
        call this.applyBCZero0_R(RHS, dirichletBC, direct)
    else
        call this.applyBCZero0_R(RHS, dirichletBC)
    end if
	call dirichletBC.empty()
end subroutine

subroutine applyBCL23_const(this, A, RHS, label, u0, v0, w0, tp, direct)
    use Set_m
	implicit none
	class(FES1DL23), intent(in) :: this
	type(smatrix), intent(inout) :: A
	real*8, intent(inout) :: RHS(:)
	integer, intent(in) :: label(:)
	real*8, intent(in) :: u0, v0, w0
	integer, intent(in) :: tp
    integer, intent(in), optional :: direct

	real*8, allocatable :: uu0(:)
	integer :: i
	real*8 :: x, y
	type(mesh1DL2), pointer :: mesh1D
    type(LinkedSet) :: dirichletBC
	type(ele), pointer:: current_p
    mesh1D => this%mesh1D

	call mesh1D.extractBoundaryNodes(dirichletBC, label)
    
    if (present(direct)) then
        allocate(uu0(dirichletBC%size))
        i = 0
        current_p => dirichletBC%head
        if (direct == directX) then
            uu0 = u0
        else if (direct == directY) then
            uu0 = v0
        else if (direct == directZ) then
            uu0 = w0
        end if
        call this.applyBC0(A, RHS, dirichletBC, uu0, tp, direct)
    else
        allocate(uu0(3 * dirichletBC%size))
        i = 0
        do i = 1, dirichletBC%size
            uu0(3 * i - 2) = u0
            uu0(3 * i - 1) = v0
            uu0(3 * i) = w0
        end do
        call this.applyBC0(A, RHS, dirichletBC, uu0, tp)
    end if
	deallocate(uu0)
	call dirichletBC.empty()
end subroutine

subroutine applyBCL23_point(this, A, RHS, node, u, v, w, tp, direct)
    use Set_m
    implicit none
    class(FES1DL23), intent(in) :: this
    type(SMatrix), intent(inout) :: A
    real*8, intent(inout) :: RHS(this%nDof)
    real*8, intent(in) :: u, v, w
    integer, intent(in) :: node, tp
    integer, intent(in), optional :: direct
    
    real*8, allocatable :: uu(:)
    integer, allocatable  :: BCDOF(:)
    if (.NOT. present(direct)) then
        allocate(BCDOF(2), uu(2))
        BCDOF(1) = 3 * node - 2
        BCDOF(2) = 3 * node - 1
        BCDOF(3) = 3 * node
        uu(1) = u
        uu(2) = v
        uu(3) = w
        call this.applyBC_DOF(A, RHS, BCDOF, uu, tp)
    else if (direct == directX) then
        allocate(BCDOF(1), uu(1))
        BCDOF(1) = 3 * node - 2
        uu(1) = u
        call this.applyBC_DOF(A, RHS, BCDOF, uu, tp)
    else if (direct == directY) then
        allocate(BCDOF(1), uu(1))
        BCDOF(1) = 3 * node - 1
        uu(1) = v
        call this.applyBC_DOF(A, RHS, BCDOF, uu, tp)
    else if (direct == directZ) then
        allocate(BCDOF(1), uu(1))
        BCDOF(1) = 3 * node
        uu(1) = w
        call this.applyBC_DOF(A, RHS, BCDOF, uu, tp)
    end if
    deallocate(BCDOF, uu)
end subroutine

end module